#include "prepare_state.h"
#include "fsm.h"
#include "main_controller.h"
#include "masters/arm_controller.h"
using namespace std;
using namespace arm;
PrepareState::PrepareState()
{

}


/**
 * @brief ReadyState::Update
 * @param fsm
 *
 */

void PrepareState::Update(FSM *fsm)
{
    //cout<<"ready state"<<endl;

    //cout<<slave_names_[fsm->arm_controller_->GetSlaveTargetIndex()]<<" PrepareState::Update"<<endl;
    switch (fsm->arm_controller_->GetRunningStatus())
    {
    case main_controller::kRCStop:
        fsm->SetState(BaseState::kSleepState);
        break;
    case main_controller::kRCWorking:
        if(fsm->arm_controller_->GetConnectionStatus()==main_controller::SocketConnectionStatusType::kConnectionOkay
                && fsm->arm_controller_->GetMasterDeviceIndex()>=0)
            fsm->SetState(BaseState::kResetState);
        break;
    default:
        break;
    }
}

void PrepareState::OnEntry(FSM *fsm)
{
    cout<<slave_names_[fsm->arm_controller_->GetSlaveTargetIndex()]<<" PrepareState::OnEntry"<<endl;
    //fsm->arm_controller_->StartRobotRC();
}

void PrepareState::OnExit(FSM *fsm)
{
    cout<<slave_names_[fsm->arm_controller_->GetSlaveTargetIndex()]<<" PrepareState::OnExit"<<endl;

}



